#ifndef GAMEENGINE_GEOMETRY_COORDINATESYSTEMS_TRANSFORMER3D_HPP
#define GAMEENGINE_GEOMETRY_COORDINATESYSTEMS_TRANSFORMER3D_HPP

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================
// Implementation of the Transformer3d class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Transformer3d<T>::Transformer3d() {
	// Setup as identity
	matrix_[0][0] = (T)1;
	matrix_[0][1] = (T)0;
	matrix_[0][2] = (T)0;
	matrix_[1][0] = (T)0;
	matrix_[1][1] = (T)1;
	matrix_[1][2] = (T)0;
	matrix_[2][0] = (T)0;
	matrix_[2][1] = (T)0;
	matrix_[2][2] = (T)1;
	matrix_[3][0] = (T)0;
	matrix_[3][1] = (T)0;
	matrix_[3][2] = (T)0;
}

//! Constructor
/*!
 *  Builds a 3D transformer that converts points from1, from2, from3 and from4 into points to1, to2, to3 and to4
 *
 @param from1 The point to transform into to1 in the destination coordinate system
 @param from2 The point to transform into to2 in the destination coordinate system
 @param from3 The point to transform into to3 in the destination coordinate system
 @param from4 The point to transform into to4 in the destination coordinate system
 @param to1 The point in which from1 should be transformed in the destination coordinate system
 @param to2 The point in which from2 should be transformed in the destination coordinate system
 @param to3 The point in which from3 should be transformed in the destination coordinate system
 @param to4 The point in which from4 should be transformed in the destination coordinate system
 @sa Function set_transformation is called by this constructor
 @note Class T should have a default constructor
 */
template <class T>
Transformer3d<T>::Transformer3d(const Point3d<T>& from1, const Point3d<T>& from2, const Point3d<T>& from3, const Point3d<T>& from4,
								const Point3d<T>& to1, const Point3d<T>& to2, const Point3d<T>& to3, const Point3d<T>& to4)
{
	set_transformation(from1, from2, from3, from4, to1, to2, to3, to4);
}

//! Copy constructor
/*!
 @param rhs The transformer to copy
 @note Class T should have a default constructor
 @note Class T should have an assignment operator
 */
template <class T>
Transformer3d<T>::Transformer3d(const Transformer3d& rhs) {
	// Copy the transformation matrix
	for ( int k = 0; k < 4; ++k ) {
		for ( int l = 0; l < 3; ++l ) {
			matrix_[k][l] = rhs.matrix_[k][l];
		}
	}
}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Transformer3d<T>::~Transformer3d() {}

//! Assignment operator
/*!
 @param rhs The transformer to copy
 @note Class T should have an assignment operator
 */
template <class T>
Transformer3d<T>& Transformer3d<T>::operator=(const Transformer3d& rhs) {
	if ( &rhs != this ) {
		// Copy the transformation matrix
		for ( int k = 0; k < 4; ++k ) {
			for ( int l = 0; l < 3; ++l ) {
				matrix_[k][l] = rhs.matrix_[k][l];
			}
		}
	}
	return *this;
}

//! Setup a three-dimensional transformation
/*!
 @param from1 The point to transform into to1 in the destination coordinate system
 @param from2 The point to transform into to2 in the destination coordinate system
 @param from3 The point to transform into to3 in the destination coordinate system
 @param from4 The point to transform into to4 in the destination coordinate system
 @param to1 The point in which from1 should be transformed in the destination coordinate system
 @param to2 The point in which from2 should be transformed in the destination coordinate system
 @param to3 The point in which from3 should be transformed in the destination coordinate system
 @param to4 The point in which from4 should be transformed in the destination coordinate system
 @return True if such a transformer exists. If no such transformation can be setup, the transformer is not modified.
 @note Class T should have an assignment operator
 */
template <class T>
bool Transformer3d<T>::set_transformation(const Point3d<T>& from1, const Point3d<T>& from2, const Point3d<T>& from3, const Point3d<T>& from4,
								const Point3d<T>& to1, const Point3d<T>& to2, const Point3d<T>& to3, const Point3d<T>& to4)
{
	// Compute denominator
	T den =  ( -from2.z()*from3.y()*from4.x() + from2.y()*from3.z()*from4.x()
			  + from2.z()*from3.x()*from4.y() - from2.x()*from3.z()*from4.y()
			  + from1.z()*(from2.y()*(from3.x() - from4.x()) + from3.y()*from4.x()
			  - from3.x()*from4.y() + from2.x()*(-from3.y() + from4.y()))
			  - from2.y()*from3.x()*from4.z() + from2.x()*from3.y()*from4.z()
			  + from1.y()*(-from3.z()*from4.x() + from2.z()*(-from3.x() + from4.x())
			  + from2.x()*(from3.z() - from4.z()) + from3.x()*from4.z())
			  + from1.x()*(from2.z()*(from3.y() - from4.y()) + from3.z()*from4.y()
			  - from3.y()*from4.z() + from2.y()*(-from3.z() + from4.z())));
	if ( den == (T)0 ) return false;

	// Setup matrix
	matrix_[0][0] = ( (from1.z()*from4.y() - from1.y()*from4.z())*(to2.x() - to3.x())
		+ from3.z()*(from4.y()*(to1.x() - to2.x()) + from1.y()*(to2.x() - to4.x()))
		+ from2.y()*(from4.z()*(to1.x() - to3.x()) + from1.z()*(to3.x() - to4.x())
		+ from3.z()*(-to1.x() + to4.x())) + from3.y()*(from4.z()*(-to1.x() + to2.x())
		+ from1.z()*(-to2.x() + to4.x())) + from2.z()*(from4.y()*(-to1.x() + to3.x())
		+ from3.y()*(to1.x() - to4.x()) + from1.y()*(-to3.x() + to4.x())) ) / den;
	matrix_[1][0] = (-(from1.z()*from4.x() - from1.x()*from4.z())*(to2.x() - to3.x())
		+ from3.x()*(from4.z()*(to1.x() - to2.x()) + from1.z()*(to2.x() - to4.x()))
		+ from2.z()*(from4.x()*(to1.x() - to3.x()) + from1.x()*(to3.x() - to4.x())
		+ from3.x()*(-to1.x() + to4.x())) + from3.z()*(from4.x()*(-to1.x() + to2.x())
		+ from1.x()*(-to2.x() + to4.x())) + from2.x()*(from4.z()*(-to1.x() + to3.x())
		+ from3.z()*(to1.x() - to4.x()) + from1.z()*(-to3.x() + to4.x())) ) / den;
	matrix_[2][0] = ( (from1.y()*from4.x() - from1.x()*from4.y())*(to2.x() - to3.x())
		+ from3.y()*(from4.x()*(to1.x() - to2.x()) + from1.x()*(to2.x() - to4.x()))
		+ from2.x()*(from4.y()*(to1.x() - to3.x()) + from1.y()*(to3.x() - to4.x())
		+ from3.y()*(-to1.x() + to4.x())) + from3.x()*(from4.y()*(-to1.x() + to2.x())
		+ from1.y()*(-to2.x() + to4.x())) + from2.y()*(from4.x()*(-to1.x() + to3.x())
		+ from3.x()*(to1.x() - to4.x()) + from1.x()*(-to3.x() + to4.x())) ) / den;
	matrix_[3][0] = ( (from1.z()*from3.y()*from4.x() - from1.y()*from3.z()*from4.x()
		- from1.z()*from3.x()*from4.y() + from1.x()*from3.z()*from4.y() + from1.y()*from3.x()*from4.z()
		- from1.x()*from3.y()*from4.z())*to2.x() + from2.z()*(-from3.y()*from4.x()*to1.x()
		+ from3.x()*from4.y()*to1.x() + from1.y()*from4.x()*to3.x() - from1.x()*from4.y()*to3.x()
		- from1.y()*from3.x()*to4.x() + from1.x()*from3.y()*to4.x()) + from2.y()*(from3.z()*from4.x()*to1.x()
		- from3.x()*from4.z()*to1.x() - from1.z()*from4.x()*to3.x() + from1.x()*from4.z()*to3.x()
		+ from1.z()*from3.x()*to4.x() - from1.x()*from3.z()*to4.x()) + from2.x()*(-from3.z()*from4.y()*to1.x()
		+ from3.y()*from4.z()*to1.x() + from1.z()*from4.y()*to3.x() - from1.y()*from4.z()*to3.x()
		- from1.z()*from3.y()*to4.x() + from1.y()*from3.z()*to4.x()) ) / den;
	matrix_[0][1] = ( (from1.z()*from4.y() - from1.y()*from4.z())*(to2.y() - to3.y())
		+ from3.z()*(from4.y()*(to1.y() - to2.y()) + from1.y()*(to2.y() - to4.y()))
		+ from2.y()*(from4.z()*(to1.y() - to3.y()) + from1.z()*(to3.y() - to4.y())
		+ from3.z()*(-to1.y() + to4.y())) + from3.y()*(from4.z()*(-to1.y() + to2.y())
		+ from1.z()*(-to2.y() + to4.y())) + from2.z()*(from4.y()*(-to1.y() + to3.y())
		+ from3.y()*(to1.y() - to4.y()) + from1.y()*(-to3.y() + to4.y())) ) / den;
	matrix_[1][1] = (-(from1.z()*from4.x() - from1.x()*from4.z())*(to2.y() - to3.y())
		+ from3.x()*(from4.z()*(to1.y() - to2.y()) + from1.z()*(to2.y() - to4.y()))
		+ from2.z()*(from4.x()*(to1.y() - to3.y()) + from1.x()*(to3.y() - to4.y())
		+ from3.x()*(-to1.y() + to4.y())) + from3.z()*(from4.x()*(-to1.y() + to2.y())
		+ from1.x()*(-to2.y() + to4.y())) + from2.x()*(from4.z()*(-to1.y() + to3.y())
		+ from3.z()*(to1.y() - to4.y()) + from1.z()*(-to3.y() + to4.y())) ) / den;
	matrix_[2][1] = ( (from1.y()*from4.x() - from1.x()*from4.y())*(to2.y() - to3.y())
		+ from3.y()*(from4.x()*(to1.y() - to2.y()) + from1.x()*(to2.y() - to4.y()))
		+ from2.x()*(from4.y()*(to1.y() - to3.y()) + from1.y()*(to3.y() - to4.y())
		+ from3.y()*(-to1.y() + to4.y())) + from3.x()*(from4.y()*(-to1.y() + to2.y())
		+ from1.y()*(-to2.y() + to4.y())) + from2.y()*(from4.x()*(-to1.y() + to3.y())
		+ from3.x()*(to1.y() - to4.y()) + from1.x()*(-to3.y() + to4.y())) ) / den;
	matrix_[3][1] = ( (from1.z()*from3.y()*from4.x() - from1.y()*from3.z()*from4.x()
		- from1.z()*from3.x()*from4.y() + from1.x()*from3.z()*from4.y() + from1.y()*from3.x()*from4.z()
		- from1.x()*from3.y()*from4.z())*to2.y() + from2.z()*(-from3.y()*from4.x()*to1.y()
		+ from3.x()*from4.y()*to1.y() + from1.y()*from4.x()*to3.y() - from1.x()*from4.y()*to3.y()
		- from1.y()*from3.x()*to4.y() + from1.x()*from3.y()*to4.y()) + from2.y()*(from3.z()*from4.x()*to1.y()
		- from3.x()*from4.z()*to1.y() - from1.z()*from4.x()*to3.y() + from1.x()*from4.z()*to3.y()
		+ from1.z()*from3.x()*to4.y() - from1.x()*from3.z()*to4.y()) + from2.x()*(-from3.z()*from4.y()*to1.y()
		+ from3.y()*from4.z()*to1.y() + from1.z()*from4.y()*to3.y() - from1.y()*from4.z()*to3.y()
		- from1.z()*from3.y()*to4.y() + from1.y()*from3.z()*to4.y()) ) / den;
	matrix_[0][2] = ( (from1.z()*from4.y() - from1.y()*from4.z())*(to2.z() - to3.z())
		+ from3.z()*(from4.y()*(to1.z() - to2.z()) + from1.y()*(to2.z() - to4.z()))
		+ from2.y()*(from4.z()*(to1.z() - to3.z()) + from1.z()*(to3.z() - to4.z())
		+ from3.z()*(-to1.z() + to4.z())) + from3.y()*(from4.z()*(-to1.z() + to2.z())
		+ from1.z()*(-to2.z() + to4.z())) + from2.z()*(from4.y()*(-to1.z() + to3.z())
		+ from3.y()*(to1.z() - to4.z()) + from1.y()*(-to3.z() + to4.z())) ) / den;
	matrix_[1][2] = (-(from1.z()*from4.x() - from1.x()*from4.z())*(to2.z() - to3.z())
		+ from3.x()*(from4.z()*(to1.z() - to2.z()) + from1.z()*(to2.z() - to4.z()))
		+ from2.z()*(from4.x()*(to1.z() - to3.z()) + from1.x()*(to3.z() - to4.z())
		+ from3.x()*(-to1.z() + to4.z())) + from3.z()*(from4.x()*(-to1.z() + to2.z())
		+ from1.x()*(-to2.z() + to4.z())) + from2.x()*(from4.z()*(-to1.z() + to3.z())
		+ from3.z()*(to1.z() - to4.z()) + from1.z()*(-to3.z() + to4.z())) ) / den;
	matrix_[2][2] = ( (from1.y()*from4.x() - from1.x()*from4.y())*(to2.z() - to3.z())
		+ from3.y()*(from4.x()*(to1.z() - to2.z()) + from1.x()*(to2.z() - to4.z()))
		+ from2.x()*(from4.y()*(to1.z() - to3.z()) + from1.y()*(to3.z() - to4.z())
		+ from3.y()*(-to1.z() + to4.z())) + from3.x()*(from4.y()*(-to1.z() + to2.z())
		+ from1.y()*(-to2.z() + to4.z())) + from2.y()*(from4.x()*(-to1.z() + to3.z())
		+ from3.x()*(to1.z() - to4.z()) + from1.x()*(-to3.z() + to4.z())) ) / den;
	matrix_[3][2] = ( (from1.z()*from3.y()*from4.x() - from1.y()*from3.z()*from4.x()
		- from1.z()*from3.x()*from4.y() + from1.x()*from3.z()*from4.y()
		+ from1.y()*from3.x()*from4.z() - from1.x()*from3.y()*from4.z())*to2.z()
		+ from2.z()*(-from3.y()*from4.x()*to1.z() + from3.x()*from4.y()*to1.z()
		+ from1.y()*from4.x()*to3.z() - from1.x()*from4.y()*to3.z() - from1.y()*from3.x()*to4.z()
		+ from1.x()*from3.y()*to4.z()) + from2.y()*(from3.z()*from4.x()*to1.z()
		- from3.x()*from4.z()*to1.z() - from1.z()*from4.x()*to3.z() + from1.x()*from4.z()*to3.z()
		+ from1.z()*from3.x()*to4.z() - from1.x()*from3.z()*to4.z())
		+ from2.x()*(-from3.z()*from4.y()*to1.z() + from3.y()*from4.z()*to1.z()
		+ from1.z()*from4.y()*to3.z() - from1.y()*from4.z()*to3.z() - from1.z()*from3.y()*to4.z()
		+ from1.y()*from3.z()*to4.z()) ) / den;
	return true;
}

//! Add a translation of the given vector to the transformation matrix
/*!
 @param offset The translation vector
 @return Always return true
 */
template <class T>
bool Transformer3d<T>::translate(const Vector3d<T>& offset) {
	matrix_[3][0] += offset.x();
	matrix_[3][1] += offset.y();
	matrix_[3][2] += offset.z();
	return true;
}

//! Add a rotation of the given angle around the given vector to the transformation matrix
/*!
 @param angle The rotation angle
 @param axis The rotation axis
 @return True if rotation could be performed, false otherwise
 */
template <class T>
bool Transformer3d<T>::rotate(const Angle<T>& angle, const Vector3d<T>& axis) {
	// Normalize the axis
	if ( axis.square_length() == 0 ) return false;
	Vector3d<T> a = axis; a.normalize();

	// Get the rotation angle variables (sine and cosine)
	T c = cos(angle.rad());
	T s = sin(angle.rad());

	// Compute rotation matrix
	T mat[3][3];
	mat[0][0] = a.x()*a.x()*((T)1.0-c) + c;
	mat[0][1] = a.y()*a.x()*((T)1.0-c) + a.z()*s;
	mat[0][2] = a.x()*a.z()*((T)1.0-c) - a.y()*s;
	mat[1][0] = a.x()*a.y()*((T)1.0-c) - a.z()*s;
	mat[1][1] = a.y()*a.y()*((T)1.0-c) + c;
	mat[1][2] = a.y()*a.z()*((T)1.0-c) + a.x()*s;
	mat[2][0] = a.x()*a.z()*((T)1.0-c) + a.y()*s;
	mat[2][1] = a.y()*a.z()*((T)1.0-c) - a.x()*s;
	mat[2][2] = a.z()*a.z()*((T)1.0-c) + c;
	
	// Compute temporary matrix
	T tmp[3][3];
	for ( int k = 0; k < 3; ++k ) {
		for ( int l = 0; l < 3; ++l ) {
			tmp[k][l] = matrix_[k][l];
		}
	}

	// Multiply matrices
	for ( int k = 0; k < 3; ++k ) {
		for ( int l = 0; l < 3; ++l ) {
			matrix_[k][l] = 0;
			for ( int m = 0; m < 3; ++m ) {
				matrix_[k][l] += tmp[k][m]*mat[m][l];
			}
		}
	}

	return true;
}

//! Accessor to the given index of the transformation matrix (read only)
/*!
 @return The given index of the transformation matrix
 */
template <class T>
T Transformer3d<T>::matrix(Transformer3D::IndexRow row, Transformer3D::IndexColumn column) const {
	return matrix_[row][column];
}

//! Accessor to the given index of the transformation matrix (read-write)
/*!
 @return A reference to the given index of the transformation matrix
 */
template <class T>
T& Transformer3d<T>::matrix(Transformer3D::IndexRow row, Transformer3D::IndexColumn column) {
	return matrix_[row][column];
}

//! Transform the given point from the source space to the destination space
/*!
 @param from The coordinates of the point to transform in the source space
 @return The coordinates of the "from" point in the destination space
 */
template <class T>
Point3d<T> Transformer3d<T>::transform(const Point3d<T>& from) const {
	return Point3d<T>(
		matrix_[0][0]*from.x() + matrix_[1][0]*from.y() + matrix_[2][0]*from.z() + matrix_[3][0],
		matrix_[0][1]*from.x() + matrix_[1][1]*from.y() + matrix_[2][1]*from.z() + matrix_[3][1],
		matrix_[0][2]*from.x() + matrix_[1][2]*from.y() + matrix_[2][2]*from.z() + matrix_[3][2]
	);
}

//! Create an inverse transformer, i.e., the transformer that converts points from the destination space to the source space
/*!
 @param ok If not nil, receives true if an inverse transformer exists and false otherwise
 @return The inverse transformer. If no such transformer exists, the identity transformer is returned
 */
template <class T>
Transformer3d<T> Transformer3d<T>::inverse(bool* ok) const {
	Point3d<T> p0(0,0,0);
	Point3d<T> p1(1,0,0);
	Point3d<T> p2(0,1,0);
	Point3d<T> p3(0,0,1);
	Transformer3d res;
	bool b = res.set_transformation(transform(p0),transform(p1),transform(p2),transform(p3),p0,p1,p2,p3);
	if ( ok != nil ) *ok = b;
	return res;
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
